/*
    Copyright (c) 2011 Marcus Schwartz.  All rights reserved.
 
    This file is part of NetObjects-Arduino.

    NetObjects-Arduino is free software: you can redistribute it and/or 
    modify it under the terms of the GNU Lesser General Public License as 
    published by the Free Software Foundation, either version 3 of the 
    License, or (at your option) any later version.

    NetObjects-Arduino is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public 
    License along with NetObjects-Arduino.  If not, see 
    <http://www.gnu.org/licenses/>.
 */

#include <NetObjects.h>

/***********************************************************
 * NetObject
 */

NetObject::NetObject(void) { }

/***********************************************************
 * NetObjectsControl
 */

NetObjectsControl::NetObjectsControl(void) {
}

void NetObjectsControl::begin(int speed) {
    _xbee.begin(speed);
}

int NetObjectsControl::getRssi(void) {
    return _rssi;
}

void *NetObjectsControl::getPacket(void) {
  restart:
    _xbee.readPacket();
    if (_xbee.getResponse().isAvailable()) {
        if (_xbee.getResponse().getApiId() == RX_16_RESPONSE) {
            _xbee.getResponse().getRx16Response(_rx);
            uint8_t packet_type = _rx.getData(0);
            _rssi = _rx.getRssi();
            switch (packet_type) {
                case MOTION_DEBUG_MESSAGE:
                case MOTION_SKID_STEER_VEHICLE:
                case MOTION_TWO_BUTTON_JOYSTICK:
                    return (NetObject *) _rx.getData();
            }
            char foo[12];
            sprintf(foo, "UnTy%d", _xbee.getResponse().getApiId());
            debug(foo);
            return 0;
        } else if (_xbee.getResponse().getApiId() == TX_STATUS_RESPONSE) {
            _xbee.getResponse().getZBTxStatusResponse(_txStatus);
            // if (_txStatus.getStatus() != SUCCESS) debug("TxF");
            goto restart;
        } else {
            char foo[12];
            sprintf(foo, "UnRe%d", _xbee.getResponse().getApiId());
            debug(foo);
            goto restart;
        }
    } 
    if (0 && _xbee.getResponse().isError()) {
        char foo[10];
        sprintf(foo, "XbEr%d", _xbee.getResponse().getErrorCode());
        debug(foo);
    }
    return 0;
}

void NetObjectsControl::processUpdate(DebugMessage d) {
    if (_localDebug) {
        debug("<");
        debug(d.text);
        debug(">");
    }
}

void NetObjectsControl::debug(char *text) {
    if (_localDebug) {
        int l = strlen(_debug) + strlen(text);
        if (l > 39) {
            strcpy(_debug, _debug + (l - 39));
        }
        strcat(_debug, text);
    } else {
        DebugMessage(this, text).broadcast();
    }
}

void NetObjectsControl::sendPacket(void *data, int len, uint8_t ptype) {
    uint8_t buf[45];
    buf[0] = ptype;
    memcpy(buf + 1, data, len);
    _tx = Tx16Request(0, buf, len + 1);
    _xbee.send(_tx);
}

void NetObjectsControl::broadcast(TwoButtonJoystick *mc) {
    sendPacket(mc->packetData(), mc->packetBytes(), MOTION_TWO_BUTTON_JOYSTICK);
}

void NetObjectsControl::broadcast(DebugMessage *d) {
    sendPacket(d->packetData(), d->packetBytes(), MOTION_DEBUG_MESSAGE);
}

void NetObjectsControl::broadcast(SkidSteerVehicle *v) {
    sendPacket(v->packetData(), v->packetBytes(), MOTION_SKID_STEER_VEHICLE);
}

char * NetObjectsControl::getDebug(void) {
    return _debug;
}

void NetObjectsControl::localDebug(int l) {
    _localDebug = l;
}

NetObjectsUpdate NetObjectsControl::getUpdate(void) {
    NetObjectsUpdate mu = NetObjectsUpdate(this);
    
    void *p;
    while (p = getPacket()) {
        mu.setData((byte *) p);
        if (mu.type() == MOTION_DEBUG_MESSAGE)
            processUpdate(mu.debugMessage());
        else
            return mu;
    }
}

/***********************************************************
 * TwoButtonJoystick
 */

TwoButtonJoystick::TwoButtonJoystick(void) { }

TwoButtonJoystick::TwoButtonJoystick(NetObjectsControl *m) {
    _motion = m;
}

TwoButtonJoystick::TwoButtonJoystick(NetObjectsControl *m, void *d) {
    _motion = m;
    if (d) {
        memcpy(&vec, d, sizeof(vec));
    }
}

void TwoButtonJoystick::broadcast(void) {
    if (memcmp(&vec, &last_vec, sizeof(vec))) {
        _motion->broadcast(this);
        memcpy(&last_vec, &vec, sizeof(vec));
        skip_count = 0;
    } else if (skip_count++ > 10) {
        _motion->broadcast(this);
        skip_count = 0;
    }
}

uint8_t TwoButtonJoystick::magnitude(void) { return vec.magnitude; }
uint8_t TwoButtonJoystick::angle(void) { return vec.angle; }
uint8_t TwoButtonJoystick::flags(void) { return vec.flags; }

void * TwoButtonJoystick::packetData(void) {
    return &vec;
}

int TwoButtonJoystick::packetBytes(void) {
    return sizeof(vec);
}

void vector(int x, int y, struct motion_vector *vec) {
    vec->flags = 0;
    vec->magnitude = sqrt(sq(x) + sq(y));
    if (vec->magnitude > 100)
        vec->magnitude = 100;
                
    if (y)
       vec->angle = ( 100 * (float) atan((float)abs(x)/(float)abs(y)) / HALF_PI );
    else
       vec->angle = 100;
          
    if (vec->magnitude > 5) {
        if (y > 0)
            vec->flags |= MOTION_FWD;
        else if (y < -0)
            vec->flags |= MOTION_REV;
    } else {
        vec->magnitude = 0;
    }
             
    if (abs(x) > 5) {
        if (x > 0)
            vec->flags |= ANGLE_RIGHT;
        else if (x < 0)
            vec->flags |= ANGLE_LEFT;
    } else {
        vec->angle = 0;
    }
               
}

void TwoButtonJoystick::updateFromXY(int x, int y, int brake, int accel) {
    vector(x, y, &vec);

    if (brake)
        vec.flags |= BUTTON_C;
    if (accel)
        vec.flags |= BUTTON_Z;
}


/***********************************************************
 * DebugMessage
 */

DebugMessage::DebugMessage(NetObjectsControl *m, char *newtext) {
    _motion = m;
    strcpy(text, newtext);
    length = strlen(text);
}

DebugMessage::DebugMessage(NetObjectsControl *m, void *d) {
    _motion = m;
    if (d) {
        uint8_t *l;
        l = (uint8_t *) d;
        length = *l;
        memcpy(&text, (uint8_t *) d + 1, length);
        text[length] = '\0';
    }
}

void DebugMessage::broadcast(void) {
    _motion->broadcast(this);
}

void * DebugMessage::packetData(void) {
    return &length;
}

int DebugMessage::packetBytes(void) {
    return 1 + strlen(text);
}

/***********************************************************
 * SkidSteerVehicle
 */

void SkidSteerVehicle::maxSpeed(uint8_t s) {
    _max_speed = s;
}

SkidSteerVehicle::SkidSteerVehicle(void) { 
    _max_speed = 255;
}

SkidSteerVehicle::SkidSteerVehicle(NetObjectsControl *m) { 
    _max_speed = 255;
    _motion = m;
}

SkidSteerVehicle::SkidSteerVehicle(NetObjectsControl *m, void *d) {
    _max_speed = 255;
    _motion = m;
    if (d) {
        memcpy(&stat, d, sizeof(stat));
    }
}

void SkidSteerVehicle::broadcast(void) {
    if (memcmp(&stat, &last_stat, sizeof(stat))) {
        if (skip_count++ > 50) {
            _motion->broadcast(this);
            memcpy(&last_stat, &stat, sizeof(stat));
            skip_count = 0;
        }
    } else if (skip_count++ > 500) {
        _motion->broadcast(this);
        skip_count = 0;
    }
}

void * SkidSteerVehicle::packetData(void) {
    return &stat;
}

int SkidSteerVehicle::packetBytes(void) {
    return sizeof(stat);
}

void SkidSteerVehicle::heading(int h) { stat.heading = h; }
int SkidSteerVehicle::heading(void) { return stat.heading; }

void SkidSteerVehicle::rssi(uint8_t h) { stat.rssi = h; }
uint8_t SkidSteerVehicle::rssi(void) { return stat.rssi; }

void SkidSteerVehicle::speed_l(int h) { stat.speed_l = h; }
int SkidSteerVehicle::speed_l(void) { return stat.speed_l; }

void SkidSteerVehicle::speed_r(int h) { stat.speed_r = h; }
int SkidSteerVehicle::speed_r(void) { return stat.speed_r; }

int bounded_value(int i, int min, int max) {
  if (i < min)
    return min;
  if (i > max)
    return max;
  return i;
}

void SkidSteerVehicle::updateFromTwoButtonJoystick(TwoButtonJoystick j) {
  updateFromVector(j.magnitude(), j.angle(), j.flags());  
}

void SkidSteerVehicle::updateFromVector(uint8_t magnitude, uint8_t angle, uint8_t flags) {
  int spd = _max_speed * magnitude;

  int speed_l = spd;
  int speed_r = spd;
  
  float offset = spd;
  offset *= angle;
  offset /= 100;

  if (flags & ANGLE_LEFT)
    speed_r -= (int) offset;
  else if (flags & ANGLE_RIGHT)
    speed_l -= (int) offset;
  
  speed_r /= 100;
  speed_l /= 100;
  
  speed_l = bounded_value(speed_l, _max_speed * -1, _max_speed);
  speed_r = bounded_value(speed_r, _max_speed * -1, _max_speed);

  if (flags & MOTION_FWD) {
    speed_l *= 1;
    speed_r *= 1;
  } else if (flags & MOTION_REV) {
    if (flags & ANGLE_LEFT) {
        speed_l *= 1;
        speed_r *= -1;
    } else if (flags & ANGLE_RIGHT) {
        speed_l *= -1;
        speed_r *= 1;
    } else {
        speed_l = 0;
        speed_r = 0;
    }
  } else {
    speed_r = speed_l = 0;
  }

  stat.speed_l = speed_l;
  stat.speed_r = speed_r;
}

/***********************************************************
 * NetObjectsUpdate
 */

NetObjectsUpdate::NetObjectsUpdate(NetObjectsControl *m) {
    _motion = m;
}

uint8_t NetObjectsUpdate::type(void) {
    return _type;
}

SkidSteerVehicle NetObjectsUpdate::skidSteerVehicle(void) {
    return SkidSteerVehicle(_motion, _data);
}

DebugMessage NetObjectsUpdate::debugMessage(void) {
    return DebugMessage(_motion, _data);
}

TwoButtonJoystick NetObjectsUpdate::twoButtonJoystick(void) {
    return TwoButtonJoystick(_motion, _data);
}

void NetObjectsUpdate::setData(byte *d) {
    _type = d[0];
    _data = d + 1;
}
